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SUMMARY 

The  development  of  a  procedure  for  estimating  aircraft  dynamic  states  and  instrument 
systematic  errors  from  flight  test  measurements  is  described.  The  method  has  particular 
application  in  non-steady  performance  estimation  for  reconstructing  aircraft  flight  path 
and  in  the  estimation  of  aerodynamic  characteristics  using  the  “ equation  error"  parameter 
estimation  method.  The  state  estimator  can  be  extended  to  determine  systematic  measure¬ 
ment  errors  in  the  recorded  data ,  giving  a  set  of  data  which  is  compatible  according  to 
the  kinematic  equations  which  relate  the  measurements.  The  effectiveness  of  the  procedures 
cannot  be  specified  in  a  general  way,  since  the  results  depend  upon  the  representation  of 
the  input  and  output  noise  characteristics  and  on  the  choice  of  initial  conditions  for  a 
given  problem. 

This  note  has  been  written  to  allow  users  to  apply  the  state  estimation  procedure  to 
practical  problems.  A  description  of  the  Carlson  Square  Root  Filter  and  its  application  to 
the  kinematic  equations  of  aircraft  motion  is  given.  The  documentation  of  the  computer 
program  for  state  estimation  is  also  presented. 
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1.  INTRODUCTION 


An  important  function  of  the  Aircraft  Behaviour  Studies — Fixed  Wing  Group  of  Aero¬ 
dynamics  Division  is  the  determination  of  aircraft  aerodynamic  characteristics  from  flight  test 
measurements.  To  extend  the  present  analysis  capability,  a  method  for  aircraft  state  estimation 
has  been  developed  under  Task  D.S.T.  79/105. 

Significant  advances  have  been  made  since  1960  in  the  analysis  of  aircraft  flight  test  measure¬ 
ments  for  the  determination  of  aircraft  handling  and  performance  characteristics.  Many  of  the 
techniques  developed  during  this  period  are  now  used  as  standard  analyses  methods  in  flight 
test  centres.  One  of  these  techniques,  for  the  estimation  of  aircraft  states  and  instrument  systematic 
errors,  is  described  in  this  Note.  The  technique  has  been  programmed  on  a  digital  computer, 
and  is  intended  to  augument  methods  currently  used  at  the  Aeronautical  Research  Laboratories 
for  aircraft  parameter  estimation. 

For  the  accurate  estimation  of  aircraft  handling  and  performance  characteristics,  a  compre¬ 
hensive  knowledge  of  the  aerodynamic  forces  acting  on  the  aircraft  is  required.  A  comparison 
of  wind  tunnel  and  flight  test  methods  shows  the  contrasting  nature  of  the  two  approaches  and 
illustrates  the  particular  problems  associated  with  making  flight  test  measurements.  When 
testing  aircraft  models  in  a  wind  tunnel,  it  is  possible  to  measure  the  forces  required  directly, 
and  in  many  cases,  to  a  high  degree  of  accuracy.  In  addition,  it  is  possible  to  determine  the 
contributions  made  to  the  total  forces  by  individual  components  of  the  model.  Unfortunately, 
there  are  limitations  in  the  accuracy  with  which  the  flow  field  of  the  full  scale  aircraft  can  be 
represented  and  this  leads  to  inaccuracy  in  the  prediction  of  full  scale  aircraft  characteristics. 
With  the  flight  testing  of  aircraft,  it  is  only  practicable  to  determine  the  total  aerodynamic  forces, 
and  these  are  calculated  indirectly  from  measurements  of  aircraft  motion.  The  aerodynamic 
forces  are  then  determined  mathematically  from  a  knowledge  of  the  equations  (or  mathematical 
model)  relating  the  forces  and  motion  of  the  aircraft.  Often,  the  motion  variables  which  can  be 
measured  adequately  are  not  the  variables,  called  state  variables,  which  characterise  the  system 
state  or  condition  at  any  instant.  In  these  cases  additional  algebraic  relations  are  required  in 
the  model.  During  the  analysis,  inaccuracies  within  the  measurement  can  lead  to  large  inaccu¬ 
racies  in  the  estimated  forces.  Until  recent  years,  simple  mathematical  models  have  been  used 
for  flight  test  analysis  and  the  flight  test  manoeuvres  have  been  limited  to  a  number  of  steady 
flight  conditions,  for  example,  steady  level  or  steady  turning  flight.  By  assuming  simple  flight 
conditions,  analysis  errors  are  reduced,  but  the  difficulty  of  satisfying  the  assumption  of  steady 
flight  in  practice,  introduces  additional  modelling  errors.  The  complete  description  of  forces 
acting  on  an  aircraft  is  taken  as  the  aggregate  of  the  results  from  a  number  of  different  tests. 

The  flight  test  methods  which  have  recently  been  developed  address  the  general  motion  of 
an  aircraft  and  therefore  use  a  more  comprehensive  mathematical  model  to  determine  the 
aerodynamic  forces.  The  general  approach  to  flight  test  measurement  is  known  as  system  identi¬ 
fication  and  grew  from  developments  in  modern  control  theory,  from  existing  theories  of  statistical 
inference  and  was  made  possible  by  the  availability  of  high  speed  digital  computers  and  modern 
flight  data  recording  systems.  System  identification  techniques  permit  a  greater  number  of 
aerodynamic  quantities  to  be  estimated  simultaneously,  reduce  test  time  significantly  compared 
with  steady  state  testing  techniques,  generally  produce  improvements  in  the  accuracy  of  estimated 
results,  and  provide  information  on  the  accuracy  of  the  estimates. 

Identification  has  been  defined  as  the  determination,  based  on  input  and  output  measure¬ 
ments  of  a  system  (or  model)  to  which  the  system  under  test  is  equivalent,  and  involves  the 
following  three  stages.  Firstly,  characterisation  of  a  mathematical  model  to  describe  the  system. 
Secondly,  estimation  of  the  values  of  parameters  used  in  the  model;  this  stage  may  require 
estimation  of  the  system  states  from  the  measured  inputs  and  outputs.  Finally,  it  is  necessary 
to  verify  that  the  results  obtained  are  consistent  with  the  known  physical  characteristics  of  the 
system.  As  the  range  of  applications  widens  increasing  research  effort  is  being  given  to  the 


important  first  stage  of  model  characterisation.  A  number  of  parameter  estimation  techniques, 
required  for  stage  two,  have  been  developed  and  used  successfully  on  a  wide  range  of  flight 
vehicles.  For  certain  applications  state  estimation  methods  have  also  been  developed  and  used. 
In  this  note,  a  digital  computer  program  for  system  state  estimation  is  described.  The  method 
has  been  developed  to  augment  the  methods  currently  used  for  system  identification. 

The  estimation  of  system  states  in  the  presence  of  process  and  measurement  noise  is  achieved 
essentially  by  the  Kalman  Filter  first  proposed  in  I960  in  Ref.  I.  The  filter  was  derived  assuming 
a  linear  (not  necessarily  stationary)  dynamic  system  in  which  the  system  outputs  and  system 
states  are  also  linearly  related. 

Following  the  publication  of  Ref.  1,  the  concepts  of  prediction,  filtering  and  smoothing 
were  introduced.  These  headings  describe  respectively  the  estimation  of  the  state  from  measure¬ 
ments  made  prior  to,  coincident  with  or  subsequent  to  the  time  considered.  Different  forms  of 
smoothing  can  be  used  and  these  can  be  derived  from  the  Kalman  Filter  equations.  Augmenting 
the  Kalman  Filter  with  a  smoothing  process  will  improve  the  accuracy  of  the  state  estimation 
but  will  introduce  additional  computation.  The  selection  of  an  appropriate  smoothing  method 
for  the  estimation  of  aircraft  dynamic  states  and  instrument  bias  errors  is  discussed  in  Section  3. 
State  estimation  of  nonlinear  dynamic  systems  can  be  realised,  as  discussed  in  Refs  2  and  3,  by 
linearising  the  system  state  and  measurement  equations  around  the  best  estimates  of  states  at 
each  data  point.  Estimation  of  measurement  bias  errors  or  system  parameters  can  also  be  realised 
by  the  filter  by  augmenting  the  state  vector  with  the  unknown  parameters.  When  applied  to 
nonlinear  equations,  the  filter,  is  known  as  the  Extended  Kalman  Filter.  Refs  4  and  5  have 
demonstrated  that  Square  Root  Filters  have  better  numerical  properties  and  give  more  accurate 
results  than  the  Kalman  Filter.  The  Square  Root  Filter  is  essentially  a  Kalman  Filter  in 
which  the  square  root  of  the  state  covariance  matrix,  rather  than  the  matrix  itself,  is 
propagated. 

State  estimation  methods  employing  Kalman  and  Square  Root  Filters  have  been  used 
successfully  in  the  estimation  of  aircraft  performance  characteristics.  Refs  3  and  6,  and  also  for 
estimating  aircraft  parameters  from  flight  test  data  containing  significant  process  noise.  Refs  7 
and  8.  In  certain  cases.  Refs  2  and  9,  the  Extended  Kalman  Filter  has  been  used  to  estimate 
additional  parameters  such  as  systematic  measurement  errors  and  aerodynamic  coefficients. 
However,  as  noted  in  Ref.  10,  the  use  of  the  Extended  Kalman  Filter  for  estimating  parameters 
has  not  received  general  acceptance. 

In  this  Note  a  state  estimation  program  is  described  which  can  be  used  for  estimating  both 
aircraft  dynamic  states  and  systematic  measurement  errors.  Prior  to  running  the  program  a 
selection  can  be  made  which  allows  state  estimation  of  the  full  set  of  motion  variables  or  of  the 
following  subsets:  longitudinal  motion  with  speed  variable;  longitudinal  motion  with  speed 
constant  and  lateral  motion.  A  further  selection  introduces  the  augmented  state  vector  to  permit 
estimation  of  specified  systematic  measurement  errors.  The  program  can  also  be  used  for  recon¬ 
structing  certain  flight  records  which  may  have  been  lost  due  to  instrumentation  malfunction  or 
signal  limiting. 

The  model  used  for  state  estimation  comprises  the  nonlinear  kinematic  equations  describing 
aircraft  motion  as  formulated  in  Ref.  2.  Measurements  of  aircraft,  position,  velocity  and 
acceleration  provide  the  inputs  and  outputs  to  the  model.  The  model  states  which  are  augmented 
by  systematic  measurement  errors  are  estimated  using  the  Carlson  Square  Root  Filter  developed 
in  Ref.  4. 

As  discussed  above  the  state  estimation  program  can  be  applied  to  a  number  of  different 
flight  test  analysis  tasks.  For  each  application  the  results  depend  upon  the  selection  of  initial 
conditions  and  of  the  statistics  of  the  process  and  measurement  noise.  Methods  for  selecting 
these  values  are  still  under  development,  e  g.  Ref.  13.  For  these  reasons  it  is  not  possible  to 
evaluate  the  effectiveness  of  the  procedures  in  a  general  way.  The  purpose  of  this  Note  is,  firstly, 
to  describe  the  application  of  a  square  root  filter  to  the  determination  of  aircraft  dynamic  states 
using  the  nonlinear  kinematic  equations  of  aircraft  motion;  and,  secondly,  to  document  and 
describe  the  operation  of  the  state  estimation  computer  program  so  that  it  can  be  used  and 
developed  for  the  practical  applications  previously  described. 

Section  2  of  this  Note  discusses  the  formulation  of  the  model,  which  is  based  on  the 
kinematic  equations  of  aircraft  motion  and  Section  3  describes  the  identification  technique 
employed.  Section  4  describes  the  organisation  of  the  computer  program  and  Section  5  presents 


notes  on  the  verification  of  the  correct  operation  of  the  computer  program.  In  Section  6  notes 
are  provided  on  the  use  of  the  computer  program. 


2.  MATHEMATICAL  MODEL 

The  mathematical  model  used  for  state  estimation  comprises  the  set  of  nonlinear  kinematic 
equations  relating  the  position,  velocity  and  acceleration  of  a  moving  rigid  body  with  reference 
to  a  set  of  flat  earth  axes.  The  nine  equations  for  body  linear  velocity,  Euler  angles  and  linear 


position,  which  can  be  considered  to  be  exact,  are: — 

u  —  —qw  +  rv  +  ax  —  g  sin  8  (I) 

v  =  —  ru  +  pw  +  ay  +  g  cos  6  sin  <f>  (2) 

w  =  qu  —  pv  +  az  +  g  cos  8  cos  <f>  (3) 

=  p  +  q  sin  <f>  tan  8  +  r  cos  tan  8  (4) 

6  =  q  cos  <f>  —  r  sin  <j>  (5) 

if/  —  q  sin  <f>jcos  8  -f  r  cos  <f>jcos  8  (6) 

Xb  —  u  cos  8  cos  4>  +  »(sin  <f>  sin  8  cos  —  cos  <f>  sin  </>) 

+  M>(cos  i sin  6  cos  if>  +  sin  <f>  sin  >fi)  (7) 

jib  =  u  cos  4>  sin  i/i  +  r(sin  <f>  sin  8  sin  ^  +  cos  <j>  cos  <!>) 

+  u’(cos  <f>  sin  6  sin  <fi  —  sin  <j>  cos  ifi)  (8) 

h  =  —u  sin  8  +  v  cos  8  sin  <j>  +  w  cos  8  cos  <t>  (9) 


Formulation  of  the  state  equations  comprising  the  set  of  state  variables,  input  variables 
and  output  variables  follows  that  given  in  Ref.  2. 

The  input  variables  are  taken  to  be  the  aircraft  centre  of  gravity  acceleration  components 
ax,  ay  and  az  and  the  body  rotational  rate  components  p,  q  and  r. 

The  output  variables  are  flight  path  velocity  V ,  sideslip  sensor  angle  j3„,  incidence  sensor 
angle  av,  the  aircraft  Euler  angles  <j>,  6,  and  0  and  aircraft  altitude  h  —  —Zb.  For  flight  dynamic 
testing,  the  horizontal  displacements  Xb  and  ><,  are  generally  of  no  interest. 

The  system  state  variables  are  the  aircraft  velocity  components  u ,  v,  and  w,  the  aircraft 
Euler  angles  <f>,  0  and  and  aircraft  altitude  h. 

Providing  that  the  input  measurements  are  available,  then  by  straightforward  integration 
of  Eqns  (1)  to  (9)  the  state  variables  X ,  and  hence  the  output  variables  Y  can  be  estimated. 
This  estimate  can  be  improved  by  use  of  the  Kalman  Filter  if  one  or  more  of  the  output  variables 
are  available.  The  filter  provides  an  optimal  estimate  X  of  the  state  X  based  on  the  noise  statistics 
of  the  state  variables,  including  input  noise  characteristics,  and  of  the  output  measurements. 
In  addition  to  the  random  noise,  the  input  and  output  measurements  also  contain  systematic 
errors,  due  to  instrument  bias  and  scale  errors.  It  is  assumed  therefore,  that  each  measurement 
can  be  expressed  as: 

Z  —  (1  T  hy)y  *+■  by  ■’  fly  (10) 

where  y  is  the  true  value  of  the  output; 

Av  is  an  unknown  scale  factor  error; 
by  is  an  unknown  bias  error; 
tiy  is  the  measurement  noise. 

The  term  \v  can  be  used  to  represent  an  error  in  the  first  order  coefficient  of  a  transducer 
calibration  curve.  It  can  also  be  used  to  account  for  errors  in  the  measurement  of  airspeed  (F), 
sideslip  angle  (/8)  and  incidence  angle  (a)  because  of  differences  between  the  local  flow  at  the 
sensors  and  the  free  stream  flow.  In  the  formulation  of  the  equations  given  below,  it  will  be 
assumed  that  the  instrument  calibrations  are  accurate  except  for  an  unknown  bias  error  by. 
The  errors  in  F,  /8,  and  a  due  to  local  flow  conditions  at  the  sensors  will  be  assumed  to  be  linear 
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and  will  be  represented  by  A,,  Xx,  Xg.  The  total  number  of  unknown  parameters,  comprising 
the  instrument  bias  errors  and  scale  factor  errors,  is  sixteen. 

Replacing  the  input  variables  in  the  state  equations  by  their  measured  values,  Zb,  gives 
the  following  set  of  equations: 
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Positive  at  is  defined  in  the  negative  z  direction  to  be  consistent  with  flight  test  and  instru¬ 
mentation  convention. 

The  output  equations  take  the  form: 


v  —  (I  +  A„)  \/(“2  +  1,2  +  K’2)  +  bv 
h  =  (I  +  A,)  tan-^-±^-^-^  ~  (PB  ~  bp)z^ 


*i:  =  (1  -t-  A3)  tan- 
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Substituting  the  measured  values  into  the  state  equations  introduces  process  noise,  which 
in  practice,  is  both  non-stationary  and  cross  correlated.  To  permit  application  of  the  Kalman 
Filter,  it  is  assumed  that  the  cross  correlation  is  zero  and  that  the  process  noise  is  purely  random, 
with  zero  mean  but  with  time  varying  covariance.  It  is  also  assumed  that  the  output  measure¬ 
ments  do  not  include  the  effects  of  atmospheric  turbulence.  In  addition  the  random  errors  in 
the  measured  rotational  rates  have  been  neglected  in  the  corrections  for  sensor  location  in  the 
Pv  and  a(.  output  equations. 

The  general  form  of  the  system  equations  can  be  written: 


state  equation 

X  —  f(X,  Ve,  6,  t)  +  w(t) 

(13) 

output  equation 

Y  =  h(X,  VE,  0) 

(14) 

measurement  equation 

ZE  =  Y  +  »,{/) 

(15) 

w(t)  and  n,(f)  denote  the  system  process  noise  and  the  output  measurement  noise  respectively. 
For  the  system  equations  under  consideration  it  is  shown  in  Appendix  A  that  a <(r )  is  due 
predominantly  to  the  input  measurement  noise  n,v(l)- 

For  the  complete  set  of  equations,  the  system  vectors  are: 

state  vector  XT  =  [«,  r. 

**\  h,  <f>,  8,  t/i] 

input  vector  Vet  =  (ozt. 

avc  Pe,  qs.  rg] 

output  vector  YT  =  [F,  p 

r,  *r,  h,  4>,  8,  |/|] 

state  vector 
input  vector 
output  vector 
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p — 


measurement  vector  Zet  =  [  Ve,  /3l£,  ot,.£,  hs,  4>e,  8e,  >I>e] 
vector  of  unknown  parameters 

^  br>  by,  bp, 

b&,  bp,  bp,  bp,  bp,  ^y,  kp,  Aa] 

A  reduced  set  of  equations  and  number  of  states  to  be  estimated  can  be  selected,  depending  on 
the  input  measurements  available. 


3.  IDENTIFICATION  TECHNIQUE 

Estimation  of  the  states  and  unknown  parameters  of  the  dynamic  system  described  by 
Eqns  (13),  (14)  and  (15)  is  achieved  by  using  a  square  root  formulation  of  the  Kalman  Filter, 
with  an  extension  to  account  for  system  nonlinearities. 

3.1  The  Kalman  Filter 

For  a  linear  system,  represented  by  a  set  of  discrete  equations,  an  estimation  of  the  state 
and  state  covariance  can  be  obtained  from  the  following  equations.  The  discrete  state  equation 
for  the  system  is: 

*(A+1)  =  <£(A  +  1,  A)  *(A)  +  cu(A+l)  k  —  (0,  1,2,.. .)  (16) 

where  X(k)  is  the  n  by  1  state  vector  at  the  Ath  time; 

<b(k+ 1,  k)  is  the  n  by  n  state  transition  matrix  from  the  Arth  to  the  (k+  l)th  time; 

ou(A-i- 1)  is  the  n  by  I  vector  of  process  noise  having  a  constant  value  between  the  Arth 

and  (A+l)th  time. 

It  is  assumed  that  w(k+  1)  is  a  sample  from  a  purely  random  noise  process  with  zero  mean  and 
covariance  Q(k  + 1 ).  Associated  with  the  state  equations  are  the  linearised  measurement  equations. 

ZE(k  4-1)  -  Y(k  +  I)  +  t’(A 4  1) 

=  H(k+\)X(k  +  \)  -i  r(Ar-r  1)  (A  =  0,1,2,...)  (17) 

where  Zb(A  -(•  1)  is  the  m  by  1  measurement  vector; 

Y(k- 1- 1)  is  the  m  by  1  output  vector; 

H(k~  1)  is  the  m  by  n  output  matrix; 

r(A  r  I )  is  the  m  by  I  vector  of  measurement  noise  at  the  (A- f  1  )th  time. 

The  noise  vector  i>(A+l)  is  assumed  to  be  a  sample  from  a  purely  random  measurement 
noise  process  n,(l)  and  has  zero  mean  and  diagonal  covariance  matrix  R(k  + 1). 

To  determine  an  optimal  estimate  for  X(k),  denoted  X(k),  it  is  necessary  to  provide  an 
initial  estimate  of  the  state  T(0)  and  an  initial  value  of  the  state  covariance  matrix  P(0). 

With  the  assumptions  given  above  for  the  process  noise  Q(k^  1),  the  Kalman  Filter 
propagates  the  estimated  state  ^(A  +  l)  and  state  covariance  P(k)  to  the  (k  l)th  time  according 
to  the  equations: 

X(k  •  I)  =  <£(A  f  1,  k)  X(k)  (18) 

P(k  r  I)  0(A  :  I ,k)P(k)<P(k  i  I.AF-i  Q(k  -  I)  (19) 

superscript  T  denotes  transpose. 

The  derivation  of  the  process  noise  matrix  Q{k  f  1)  is  given  in  Appendix  A. 

At  the  (A -  l)th  time  the  information  contained  in  the  measurement  vector  Ze(A+1)  is 

incorporated  by  computing  the  optimal  gain  matrix  K(k+  1)  according  to  the  following  equation 

A'(A  •  I)  P(k  ■  \)H{k  -  l)T  [H(k  >  I )  P{k  i  I)  H(A  f  1)T  f  /?(A-j  1)]  1  (20) 

superscript  -  1  denotes  matrix  inversion. 


5 


The  derivation  of  the  measurement  noise  matrix  R(k- f  I)  is  given  in  Appendix  A. 

By  using  the  optimal  gain,  updated  values  of  the  state  and  covariance  are  calculated  from: 

jf(A-l)  X(k  •  I)  r  K(k  •  I)  [ZE(k  ,  I)  -  H(k l)X'(A:  I)]  (21) 

P  (A  +  l)  -  P(k  •  1)  -  K(k  f- 1)  H(k-\  \)P(ki  1)  (22) 

superscript  -f  denotes  values  updated  by  incorporating  the  measurements  at  the  (A  -i-l)th  time. 

The  procedure  described  by  Eqns  18  to  22  is  illustrated  in  the  following  diagram: 


The  accuracy  of  the  estimates  from  the  Kalman  Filter  depends  strongly  upon  the  values 
specified  for  the  process  noise  covariance  Q(k  -f  I )  and  the  measurement  noise  covariance 
R(k+\).  To  assist  in  selecting  appropriate  values,  use  can  be  made  of  the  output  residuals, 
v  which  are  defined  as  the  difference  between  the  measured  variables,  Ze,  and  the  predicted 
measurements  Y.  That  is:  v(k  +  I)  =  ZE(k  f  I)  —  Y(k\- 1).  (23) 

As  discussed  in  Ref.  2,  with  the  correct  values  of  process  and  measurement  noise,  and 
assuming  that  the  system  is  modelled  correctly,  then  the  residuals  should  approach  a  random 
Gaussian  white  sequence  with  zero  mean  and  a  variance  consistent  with  the  values  calculated 
from  the  filter,  Eqn  (20),  i.c.: 

E{v(k  •  1)  v(k  -  I)7'1,  -  H(k  Dm  •  I)  //(A  •  \)T  -  R(k  H)  (24) 

Since  the  matrix  P(k  ■  1)  includes  the  effect  of  the  process  noise,  then  a  comparison  of  the 
calculated  residuals  from  Eqn  (23)  and  their  expected  variance  from  Eqn  (24)  can  be  used  to 
adjust  the  initial  choice  of  process  and  measurement  noise,  which  are  not  known  a  priori. 

3.2  The  Carlson  Square  Root  Formulation  of  the  Kalman  Filter 

A  practical  problem  in  the  use  of  Eqn  (22)  is  discussed  in  Refs  4  and  5.  By  definition.  P(k) 
is  positive  definite  since  P(k)  E{X{k)  %{k)T)  and  Eqns  (18)  to  (22)  will  propagate  a  positive 
definite  matrix;  however,  because  of  the  finite  word  length  of  computers,  the  matrix  subtraction 
in  Eqn  (22)  often  yields  a  non-positive  unsymmetric  matrix  after  propagation  through  a  number 
of  time  points.  This  results  in  a  non-optima!  estimate  for  %(k)  which  can  diverge  from  realistic 
values.  In  Ref.  4  Carlson  has  developed  an  algorithm  for  propagating  the  square  root  of  the 
state  covariance  matrix  5(A  )  where 

5(A  )  S(k)T  -  P(k )  (25) 

This  procedure  ensures  that  P(k)  cannot  be  unsymmetric  or  indefinite.  The  algorithm  has 
been  evaluated  in  Ref.  5  and  has  been  shown  to  give  greater  precision  than  the  Kalman  Filter 
Eqns  ( 18)  to  (22). 


The  Carlson  Filter  has  been  formulated  to  minimise  run  time  and  computer  storage  by 

ensuring  that  the  square  root  of  the  state  covariance  matrix  S(k)  remains  triangular  during 

measurement  updating.  By  careful  programming,  as  detailed  in  Ref.  4,  the  computational  speed 
is  shown  to  be  better  than  most  square  root  filters  and  comparable  with  the  speed  of  the  Kalman 
Filter.  Details  of  the  computer  program  for  the  Carlson  Filter  are  given  in  Section  4. 

In  the  Carlson  Filter,  Eqn  (19),  which  is  used  for  the  time  updating  of  P(k ),  is  replaced 
by  the  following  equations: 

H'(k  ;  1)  —  4>(k  :  I,  k)  S  (k)  (26) 

S(k  •  I)  [im+l)  mk  i  l)r  I  Q(k  ,  1)1*  (27) 

where  [  ]*  denotes  formation  of  the  square  root  matrix. 

in  the  Carlson  Filter  a  Choleski  decomposition  is  used  to  generate  S(k  *-1)  in  upper  triangular 
form.  This  can  be  accomplished  with  a  finite  procedure  since  the  state  covariance  matrix  is 
positive  definite. 

In  the  computer  program,  the  upper  triangular  matrix  S(k  ;  I)  is  stored  columnwise  and 
accessed  as  a  vector.  <t>(k  ■  I),  W'(k  ■  I)  and  Q(k  fl),  although  square  »  by  n  matrices,  are 
partitioned  and  only  the  segments  containing  non-zero  elements  are  stored. 

The  equations  for  measurement  updating  in  the  Kalman  Filter,  Eqns  (20)  to  (22)  are 
replaced  by  the  following  equations: 
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*«,  %t,  a t  and  a  for  (1  <  i  <  n)  are  scalars: 

bn(ki  I)  and  b, (k  t  1)  for  (1  <  i  <  n)  are  n  dimensional  vectors; 

fi(k  -H)  is  the  r'th  component  of  f(k  4  !): 

S/(k  rl)  is  the  rth  column  of  S(k  •  I); 

Hj(k  r  1)  is  the y'th  row  of  H(k  i  1): 

Rn(k  ;  1)  is  the  y'th  diagonal  element  of  R(k  ■  I); 

Zp^k  i  I)  is  the  /th  component  of  Zn(k  1  I), 

±Z(k  ■  I )  is  a  scalar. 

The  updated  matrix  S(k  >  I)  remains  upper  triangular  but  this  does  not  eliminate  the 
need  for  the  Choleski  decomposition  unless  the  process  noise  Q(k  ■  I),  which  is  added  in 
Eqn  (27),  is  zero. 
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3.3  Estimation  of  System  Parameters 

To  extend  the  estimation  technique  to  include  the  systematic  error  terms  in  Eqns  (11) 
and  (12),  the  system  state  vector  X  is  augmented  with  the  vector  of  unknown  parameters  6 
giving  the  augmented  state  vector 


This  extension  introduces  non-linearities  into  the  system  equations  which  already  have 
intrinsic  non-linearities  from  the  kinematic  equations.  To  estimate  the  augmented  state  vector 
for  the  set  of  non-linear  equations,  the  Extended  Kalman  Filter  is  used.  In  this  approach  the 
system  state  and  output  equations  are  linearised  about  the  best  estimate  of  the  state  at  each 
data  point  as  described  in  Appendix  B. 

The  state  and  output  equations  (Eqns  (13)  and  (14)]  are  changed  to: 

>•  rvi'iTi 

Y  =  h(XA,  VE)  (42) 

Time  updating  of  the  system  state  [given  by  Eqn  (18)  for  the  linear  case]  is  now  carried  out 
by  numerical  integration  of  Eqn  (41). 

The  state  transition  matrix  required  for  updating  of  the  state  covariance  matrix  by  Eqn  (19) 
for  the  Kalman  Filter,  or  by  Eqn  (26)  for  the  Carlson  square  root  formulation,  is  obtained  by 
linearising  Eqn  (41)  as  shown  in  Appendix  B. 

From  Eqn  (41)  it  can  be  seen  that  the  appended  state  variables  9  and  their  associated 
covariances  will  remain  unaltered  during  time  updating.  This  will  have  the  following  implications 
on  the  choice  of  initial  conditions.  Measurement  updating  in  the  Kalman  Filter  is  carried  out 
by  Eqns  (20)  to  (22). 

Substituting  Eqn  (20)  into  Eqn  (22)  gives: 

P(k~  1)  = 

P(k  -I)  -  />(*+  \)H(k  -rl)r[W(*+l)  P{k+l)H(k+  l)r  +  R(k  + 1)]  1  //(fc-fl)  P(k+ 1) 

(43) 

which  can  be  written 

P  (k  4  1)  >  =  P(k  +  \)  1  +  H(k+\)T  R(k+\)  1  H(k- fl)  (44) 

It  can  be  seen  from  Eqn  (44)  that  the  state  covariance  matrix  after  measurement  update  is 
never  larger  than  the  value  before  update,  since  H(k+  \)T  R(k+  1)  1  H(k+ 1)  is  at  least  a 
positive  semi-definite  matrix.  Thus  the  act  of  measurement  on  the  average,  never  increases  the 
uncertainty  in  the  knowledge  of  X(k).  Since  the  covariance  of  the  unknown  parameters  remain 
unaltered  during  the  time  updating  stage,  it  is  necessary  to  choose  initial  values  for  these 
covariances  which  are  greater  than  zero.  Selection  of  initial  conditions  for  the  filter  is  discussed 
in  Section  6. 


3.4  State  Smoothing 

The  “filtering  process”  which,  by  definition,  calculates  the  best  estimate  of  the  state  at  a 
given  time  from  all  the  measurements  up  to  that  time,  can  be  augmented  with  a  “smoothing 
process”.  The  smoothing  process  enables  estimates  X{k,k  H)  and  P(k,  fr+1)  to  be  made; 
that  is,  the  information  added  at  the  (*  H)th  time  can  be  used  to  give  an  estimate  at  the  £th 
time,  and  if  required,  at  all  previous  times.  In  Ref.  2  single-stage  smoothing  and  local  iteration 
of  the  state  estimate  X{k)  was  carried  out,  in  an  attempt  to  reduce  the  bias  which  is  inherent 
within  the  Extended  Kalman  Filter.  In  addition,  fixed  point  smoothing  was  used  to  update 
initial  conditions  after  each  pass  through  the  filter.  A  smoothing  procedure  has  not  been  included 
in  the  program  described  in  this  Note.  The  program  can  be  developed  to  include  the  appropriate 
smoothing  procedure  for  each  application. 
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4.  DESCRIPTION  OF  THE  STATE  ESTIMATION  COMPUTER  PROGRAM 


Effort  has  been  made  in  the  computer  program  to  take  advantage  of  matrix  symmetry  and 
triangularity  in  the  updating  of  the  square  root  covariance  matrix  S(k )  and  of  general  blocks  of 
zeroes  in  the  state  transition  matrix,  to  minimise  computation  and  storage  requirements.  A 
summary  of  operations  performed  by  the  program  is  given  in  Fig.  1. 

Only  the  storage  and  computation  of  the  first  i  entries  in  each  column  of  St{k)  is  required. 
In  addition,  it  is  only  necessary  to  store  the  12  elements  of  the  state  transition  matrix  since 
the  identity  matrix  is  implicit  in  Step  9.4  of  Fig.  1.  Numerical  integration  of  the  state 
variables  is  carried  out  by  the  variable  step  size  integration  routine  described  in  Ref.  11.  The 
calculation  of  the  state  matrix  A  is  carried  out  during  the  integration  step;  when  only  a  subset 
of  the  equations,  for  example  the  kinematic  equations  for  longitudinal  motion,  or  when  a 
reduced  number  of  states  are  to  be  estimated,  only  the  associated  equations  are  computed  and 
integrated.  Similarly  during  measurement  updating,  calculations  are  only  performed  for  the  set 
of  output  measurements  specified  in  the  program  set-up  data. 


5.  VERIFICATION  OF  COMPUTER  PROGRAM 

The  integrity  of  the  computer  program  has  been  checked  by  successively  expanding  the 
size  of  the  system  equations  from  a  simple  set  which  is  amendable  to  analytic  solution.  Initially 
the  prediction  of  the  state  and  state  covariances  for  a  second-order  system  without  measurement 
updating,  were  compared  with  the  analytic  solutions  given  in  Ref.  12.  Measurement  updating 
was  then  added  and  the  results  were  compared  with  the  numerical  solutions  also  given  in  Ref.  12. 

Using  the  Kalman  Filter  equations  and  matrix  routines  verified  in  the  two  degrees  of  freedom 
filter,  a  state  estimation  program  was  written  with  the  kinematic  equations  for  the  motion  of  a 
body  [Eqns  (11)  and  (12)]  as  the  system  model.  The  same  system  of  equations  was  also  pro¬ 
grammed  using  the  Carlson  square  root  formulation  of  the  Kalman  Filter,  and  particular  effort 
was  made  to  minimise  computation  and  storage  requirements,  as  discussed  in  Section  4.  The 
results  from  the  Kalman  Filter  were  used  to  verify  the  scalar  algorithms  employed  in  the  Carlson 
Filter.  Finally  the  Carlson  Filter  was  expanded  to  include  the  estimation  of  systematic  errors  in 
the  input  and  output  measurements. 

Because  of  the  large  number  of  permutations  of  model  complexity  and  of  states  which  can 
be  estimated,  and  because  of  the  dependence  of  the  estimates  on  the  set-up  data,  the  general 
performance  of  the  filter  has  not  been  assessed.  Refs  3  and  6  have  demonstrated  satisfactory 
results  for  the  estimation  of  the  state  variables  of  longitudinal  motion  of  an  aircraft  and  esti¬ 
mation  of  three  instrument  bias  errors.  In  Ref.  2,  good  results  were  claimed  for  the  estimation 
of  the  lateral  response  of  aircraft  motion  with  five  states  and  ten  parameters.  So  far  satisfactory 
estimation  of  the  full  set  of  seven  states  and  twenty-three  parameters  from  measured  data  has 
not  been  demonstrated. 


6.  NOTES  ON  THE  USE  OF  THE  STATE  ESTIMATION  COMPUTER  PROGRAM 

Set  up  information  for  the  state  estimation  program  called  FILTER  is  prepared  by  running 
program  CHOICE.  A  copy  of  the  set  up  data  is  presented  in  Appendix  C.  Experience  with  the 
Kalman  Filter  shows  that  the  accuracy  of  the  filter,  particularly  when  the  augmented  state  vector 
is  included,  is  critically  dependent  upon  the  values  specified  in  the  set  up  data  for  the  initial 
state  conditions  and  the  input/output  measurement  noise. 

Stage  1  of  CHOICE  requires  selection  of  the  values  for  the  input  and  output  measurement 
noise  variances.  Initial  estimates  are  made  from  a  knowledge  of  the  precision  of  the  instru¬ 
mentation  system  producing  the  measurements.  These  estimates  can  be  refined  by  an  analysis  of 
the  residuals  following  state  estimation,  by  using  Eqns  (23)  and  (24). 

Stage  2  of  CHOICE  requires  specification  of  the  input  and  output  measurement  channels 
to  be  used.  Depending  upon  the  particular  input  measurements  available,  four  different  com¬ 
binations  of  the  filter  equations  are  available  as  shown  in  Table  1.  If  a  particular  measurement 
is  not  available,  the  status  of  that  channel  is  set  at  zero,  and  its  steady  state  value  has  to  be 


TABLE  1 

Possible  Selections  of  States  to  be  Estimated 


Value 

Longitudinal 
Equations — 
Speed  and  Height 
Constant 

Longitudinal 
Equations — 
Speed  and  Height 
Variable 

Lateral 
Equations — 
Speed  and  Height 
Constant 

Complete 
Longitudinal  and 
Lateral 
Motion 

i 

Input  Measurements  Required 

1 

i 

ax 

• 

ay 

az 

\  # 

• 

supplied.  If  no  output  measurements  are  available,  the  program  is  run  purely  as  a  state  predictor. 
As  the  number  of  output  channels  to  be  included  is  increased,  the  estimation  of  the  state  is 
improved. 

Stage  3  of  CHOICE  allows  specification  of  the  particular  states  to  be  estimated.  Based 
upon  this  selection,  only  those  states  and  associated  covariance  elements  are  computed  and 
integrated.  During  selection  of  the  states  to  be  estimated  it  is  also  necessary  to  specify  the  initial 
values  of  the  state  covariance  matrix  P(k).  Generally  it  is  sufficient  to  specify  only  the  variance 
or  leading  diagonal  of  P(k)  and  for  the  unaugmented  state  vector,  an  arbitrary  positive  definite 
choice  is  satisfactory.  When  the  augmented  state  vector  is  included,  the  initial  variances  for  the 
additional  states  should  be  greater  than  zero  as  discussed  in  Section  3.3.  Tests  on  a  second-order 
system  with  the  state  vector  augmented  by  two  bias  error  parameters,  showed  that  satisfactory 
estimates  were  only  achieved  when  the  initial  state  variances  exceeded  their  final  steady  values. 

In  addition  to  the  set-up  data,  the  program  FILTER  requires  a  data  file  of  the  measurements 
in  the  order  specified  in  the  vectors  t]  and  Ze  (see  Section  2),  and  also  the  following  information 
at  run-time: 

1.  RES  — an  integer  which,  if  greater  than  0,  instructs  the  program  to  calculate 

residuals; 

2.  TSTART  — specifies  start  time  referred  to  the  beginning  of  the  measurement  data  file; 

3.  TTOT  — specifies  the  length  of  measurement  record  to  be  filtered; 

4.  AQR  — specifies  the  data  acquisition  rate; 

5.  No.  OF  PASSES — an  integer  which  specifies  the  number  of  repeated  passes  through  the  filter. 

After  each  pass,  the  initial  values  of  the  variables  in  the  extended  part  of  the  state  vector  and 
the  variances  of  the  basic  state  variables  are  set  to  the  final  values  reached  in  the  preceding  pass. 
Initial  conditions  are  used  for  the  basic  states  and  for  the  variances  of  the  augmented  states. 
Tests  with  a  second-order  system,  which  has  the  state  vector  augmented  by  two  bias  error 
parameters,  showed  that  a  single  pass  was  sufficient.  However,  tests  on  a  three  degrees  of  freedom 
system  with  the  state  vector  augmented  by  eight  bias  states,  reported  in  Ref.  2  showed  that 
continued  improvements  in  the  estimates  were  achieved  with  up  to  four  passes  through  the  filter. 

The  output  of  program  FILTER  is  a  file  containing  the  estimated  states,  and  state  variances 
and  a  file  of  bias  errors.  The  bias  errors  are  the  average  of  the  bias  states  calculated  over  the 
final  sixty  data  points. 

The  output  files  from  program  FIL1  ER  are  used  by  program  FLIGHT  to  form  a  file  of 
compatible  data  of  all  the  measured  quantities  and  also  plotting  files  of  the  compatible  data, 
state  variables,  state  variances  or  residuals  if  this  option  has  been  requested.  The  compatible  data 
is  a  file  equivalent  to  the  file  of  measurement  data,  but  with  the  input  variables  corrected  for 
bias  errors,  and  the  output  measurements  constructed  from  the  estimated  states.  An  example  of 
the  compatible  data,  estimated  using  the  Carlson  Filter  together  with  the  measured  data  for  a  set 
of  simulated  measurements,  is  plotted  in  Fig.  2.  The  input  information  to  program  FLIGHT 
is  an  integer,  greater  that  0  if  residuals  have  been  calculated,  followed  by  start  time,  total  record 
time  and  acquisition  rate  for  the  data.  A  selection  of  the  output  files  required  for  plotting  can 
also  be  made. 

When  the  option  to  calculate  residuals  is  selected,  the  computer  program  FILTER  calculates 
the  estimated  standard  deviation  of  the  residuals  from  Eqn  (24)  and  stores  the  results  in  place 
of  the  state  variances  in  channels  7  to  13.  The  average  of  the  stored  values  is  calculated  over  the 
final  sixty  data  points  of  the  time  history  and  is  stored  with  the  file  of  bias  errors.  When  program 
FLIGHT  is  informed  that  residuals  have  been  selected,  then,  the  actual  residuals  are  calculated 
and  plot  files  of  estimated  and  calculated  standard  deviations  for  the  full  time  history  are  calcu¬ 
lated  and  stored  in  place  of  the  compatible  data  and  state  variance  information  respectively. 
A  file  named  RESID  is  also  produced  which  presents  the  average  standard  deviation  for  the 
estimated  and  calculated  residuals  calculated  over  the  final  sixty  data  points. 

When  the  option  to  calculate  residuals  is  not  selected,  the  calculations  described  above  are 
by-passed. 


7.  CONCLUDING  REMARKS 


A  state  estimation  computer  program  has  been  written  for  the  estimation  of  aircraft  dynamic 
states  and  instrument  systematic  errors  from  flight  test  measurements.  The  program  can  be  used 
with  parameter  estimation  methods  for  the  determination  of  aircraft  handling  and  performance 
characteristics.  It  has  particular  application  in  non-steady  aircraft  performance  estimation,  for 
the  reconstruction  of  aircraft  flight  path,  and  in  the  estimation  of  aerodynamic  characteristics 
in  situations  where  “equation  error"  rather  than  “output  error"  parameter  estimation  methods 
are  preferred.  The  state  estimator  can  be  extended  to  determine  measurement  bias  errors  in  the 
recorded  data,  giving  a  set  of  data  which  is  compatible  according  to  the  kinematic  equations 
which  relate  the  measurements.  Used  in  this  way,  the  filter  has  the  potential  to  check  the  correct 
operation  of  channels  of  flight  instrumentation  from  measured  data  prior  to  commencing  test 
manouevres.  The  state  estimator  can  also  be  used  to  reconstruct  certain  flight  records  which 
may  have  been  lost  due  to  instrumentation  malfunction  or  signal  limiting.  The  routines  which 
have  been  developed  for  the  state  estimation  program  can  be  used  to  modify  existing  parameter 
estimation  programs  to  include  the  effects  of  process  noise  due,  for  example,  to  atmospheric 
turbulence. 

Three  subsets  of  the  full  equations  can  be  selected  which  permit  separate  analysis  of 
longitudinal  motion,  with  or  without  constant  forward  speed  or  analysis  of  purely  lateral  motion. 
The  accuracy  of  the  state  estimates  depend  strongly  upon  the  estimated  input  and  output  noise 
characteristics  and  the  initial  conditions  specified  in  the  set-up  data.  A  rigorous  approach  to  the 
selection  of  these  quantities  has  not  yet  been  developed,  and  so  the  effectiveness  of  the  procedures 
can  only  properly  be  established  through  application  to  particular  problems.  The  present  Note 
provides  a  description  of  the  method  used  for  estimating  the  states  of  a  non-linear  model  using 
a  square  root  filter,  and  documents  a  computer  program  based  on  this  method.  For  particular 
problems,  the  following  possible  program  developments  may  be  of  benefit:  firstly,  inclusion  of 
a  smoothing  algorithm,  either  as  a  single  stage  procedure  or  for  updating  the  initial  conditions; 
secondly,  replacement  of  the  variable  step  size  integrator  with  a  fixed-step  integrator  to  reduce 
program  run  time;  thirdly,  development  of  procedures  for  the  analysis  of  residuals  to  aid  in 
the  specification  of  input  and  output  noise  statistics. 
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APPENDIX  A 

Calculation  of  the  Process  Noise  and  Measurement  Noise  Covariance  Matrices 

In  Section  3  the  discrete  Kalman  Filter  equations  are  developed  for  a  linear  system 
represented  by  the  discrete  state  equation 

X(k  (1)  =  0(*4  l,k)  X(k)  +  w(k  +  1)  *  =  (0,1,2,...)  (A.l) 

<o(*+l)  is  the  process  noise  and  accounts  for  noise  in  the  input  signal,  for  noise  within  the 
process  itself,  i.e.  unmeasured  disturbances,  and  can  also  be  used  to  account  for  differences 
between  the  model  and  the  actual  system.  For  systems  which  are  accurately  modelled  and  in 
which  the  unmeasured  disturbances  are  small,  such  as  the  kinematic  relations  represented  by 
Eqns  (13)  to  (15),  it  is  assumed  that  the  process  noise  is  predominantly  caused  by  noise  in  the 
measured  inputs  «,„(*). 

Therefore  the  process  noise 

w(k  + 1 )  =  r(k  +  I )  njk  + 1 )  (A. 2) 

and  the  process  noise  covariance  matrix 

(Hk+l)  =  I\k+l)it(k+l)rT(k+l)  (A.3) 

x(*+l)  is  a  diagonal  matrix  with  elements  equal  to  the  variances  of  the  input  noise  nj*+l). 
r(k+  1 )  is  derived  in  Appendix  B. 

The  measurement  noise  is  simply  the  noise  in  the  measured  outputs 

i’(*+l)  =  «,<*+ 1)  (A  .4) 

and  the  measurement  noise  covariance  matrix  /?(*+ 1)  is  a  diagonal  matrix  with  elements  equal 
to  the  variances  of  the  output  measurement  noise  n,{k+  1). 

In  the  program  validation  described  in  Section  5,  the  solution  for  a  continuous  process  was 
approximated  by  the  discrete  Kalman  Filter.  For  this  case,  it  is  shown  in  Ref.  12  that  the  process 
and  measurement  noise  covariances  are  determined  as  follows. 

The  state  equation  for  the  continuous  linear  system  is 

X  =  A(t)X  +  w(t)  (A. 5) 

where  w(t)  is  a  Gaussian  purely  random  process 
with  £■[(«(/)]  =  0 

£[<«(/)  cu^r)]  =  £(/)£(/- t) 

Q(t)  is  a  non-negative  definite  matrix  representing  the  integral  of  the  random  process  a >(/)  and 
8(1 -t)  is  the  Dirac  delta  function. 

Using  the  discrete  approximation  given  by  Eqns  (A.l)  to  (A.3)  and  choosing  to  make  the 
state  covariance  matrix  for  the  discrete  system  P(k)  equivalent  to  its  counterpart  P(i)  in  the 
continuous  system.  Ref.  1 2  shows  that 

X(*)  =  Q(DIM  (A.6) 

where  Ar  is  the  stepsize. 

Similarly  it  is  shown  that  the  measurement  noise  v(i)  which  is  assumed  to  be  a  Gaussian 
purely  random  process  with 

£[«(/)]  =  0 

E[Ht)  i-T(r)]  =  R(r)8(t-r) 
is  represented  in  the  discrete  system  by 

R(k)  =  /?(r)/Ar  (A.7) 


In  a  practical  situation  the  statistics  of  the  process  and  measurement  noise  are  not  usually 
known  sufficiently  accurately  prior  to  state  estimation.  This  can  lead  to  large  estimation  errors 
or  even  to  a  divergence  of  the  errors.  The  problem  is  discussed  briefly  in  Ref.  2  and  in  more 
detail  in  Ref.  13.  For  the  filter  discussed  in  this  Note  the  process  and  measurement  noise  is 
chosen  from  a  comparison  of  the  output  residuals,  as  discussed  in  Section  3.1. 


APPENDIX  B 

Linearisation  of  the  Kinematic  State  and  Output  Equations 


For  the  Extended  Kalman  Filter,  the  system  state  and  output  equations  are  linearised 
about  the  best  estimate  of  the  state  at  each  data  point  XA. 

For  the  kinematic  equations,  in  which  the  process  noise  is  assumed  to  be  due  solely  to 
noise  in  the  input  measurements,  as  discussed  in  Appendix  A,  the  upper  segment  of  the  state 
equation  [Eqn  (41)]  can  be  written 

XA  =  f(XA,  VE,t)  +  BnSO  (B.  1) 

where  B  is  a  matrix  of  input  coefficients  from  Eqn  (11). 

Neglecting  differences  of  second  order  and  higher,  equation  (B.l)  can  be  approximated  by 
the  expansion 

Jf\4=/(*j,’tB,r)  +  -^L  (Xa-Xa )  +  BnJLt)  (B.2) 

dXA\XA 

=  *A  +  MaXa  (Xa~*a)  + 

(Xa-Xa)  =  {Xa-Xa)  +  Bn  Jit) 

i.e.  \XA  =  A  \XA  +  Bnjt)  (B.3) 

The  discrete  form  of  Eqn  (B.3)  is 

AA^(*+1)  =  4KA  + 1,  A)  AAr^(A)  +  /\*+ I)  «„,(*)  (B.4) 

where 

$(*  +  !,*)  =  (B.5) 

and 

r(k  +  I)  =  J;‘  H'-'i-'1  fi(r)dT  (B.6) 

Similarly,  the  output  equation  [Eqn  (42)]  can  be  approximated  as 

Y=h(XA,VE)  +  (XA-XA)  (B.7) 

Xa 

where,  from  Section  2,  it  is  noted  that  the  random  errors  in  the  rotational  rate  inputs  have 
been  neglected  in  the  output  equations  for  /S,.  and  a,  . 


Y  = 


0  ( Xa-Xa ) 

X  A 


(Y-Y)  = 


ih 

Sx'aXa 


(Xa-Xa) 


or  in  discrete  form 


AK  =  H &Xa 
AK(H1)  =  H\XA(k  fl) 


(B.8) 

(B.9) 


As  can  be  seen  from  Fig.  1  it  is  only  necessary  to  determine  A  for  the  thirteen  by  thirteen  state 
transition  matrix  0ii,i2(k+ 1,  Ar).  Within  thecomputer  program,  only  those  elements  of  A  and  B 
which  define  the  states  selected  for  estimation  are  calculated.  Similarly  only  those  elements  of 
H  which  define  the  outputs  for  which  measurements  are  available  are  calculated. 


J 


For  the  case  when  all  twenty-three  states  in  Xa  are  to  be  estimated  and  when  all  seven 
measurement  channels  in  Zb  are  available,  the  following  differentials  are  calculated.  The 
arguments  in  the  notation  A(i,j)  used  below,  denote  the  dependent  and  independent  variables 
of  the  partial  differentials  in  the  linearised  equations  respectively.  For  the  linearised  state 
equation : 

A(u,  v )  =  (rE-br) 

A(u,  w)  =  -0 ge-bi ) 

A(u,  8)  =  —g  cos  8 
A(u,  ba ,)  =—10 
A(u,  bq)  =  w 
A(u,  br)  =  —  v 

A(v,  u)  =  —  (rE  —  br) 

A(v,  w)  =  ( pE -bP ) 

A(v,  <f>)  =  g  cos  8  cos  <j> 

A(v,  bay)  =—10 

A(v,  8)  =  —g  sin  8  sin 
A(v,  bp)  =  —w 
A(v,  br)  =  u 

A(w,  u)  =  ( qs-bq ) 

A(w,  v)  =  -(pE-bp) 

A(w,  <f>)  =  —  g  cos  8  sin  <j> 

A(w,  8)  =  —  g  sin  8  cos  <£ 

A(w,  bai)  =10 
A(w,  bp)  =  v 
A(w,  bq)  =  - u 

A(h,  u )  =  sin  8 
A(h,  v)  =  —cos  8  sin  ^ 

A(h,  h’)  =  —cos  8  cos  4> 

A(h,  </>)  =  —  v  cos  8  cos  <f>  +  w  cos  8  sin  <f> 

A(h,  8)  —  u  cos  8  +  v  sin  8  sin  <f>  +  w  sin  8  cos  <f> 

A(<f>,  <f>)  =  (c/e — bq)  cos  <f>  tan  8  —  ( rE—br )  sin  <f>  tan  8 
A(4>,  8)  =  (qE—bv)  sin  <£/cos2  8  -)-  (rE—br)  cos  <£/cos2  8 
A(<f>,  bp)  =  -10 
A(4>,  bq)  =  —sin  <f>  tan  8 
A(<f> ,  br)  —  —cos  <f>  tan  8 

A(8,  <f>)  —  (qE—bq)  sin  <f>  —  ( rE—br )  cos 
A(8 ,  bq)  —  —cos  <f> 

A(8,  br)  =  sin  <j> 

A(4>,  4>)  =  ( qE—bq )  cos  $/cos  8  —  (rE—br)  sin  <f>/cos  8 
A(<p ,  0)  —  (qE—bq)  sin  <f>  tan  8/ cos  8  4-  (rE—br)  cos  <j>  tan  8/ cos  0 
A(<l>,  bq)  —  —sin  tf>/cos  8 
A(<f>,  br)  =  —cos  0/cos  8 

For  the  linearised  output  equations: 

H(V,  u)  =  (1  +Ay)  u(u2yv2+w2)-* 

H(  V,  v)  =  (1  f  Ay)  v(u2A  v2  +  w2y* 

H(V ,  w)  =  (I  +Ay)  w(u2 -j- 1?2 + w2)  * 

H(V,bv)=  10 
H(y,Av)  =  (u2+v2+h2)* 


denoting /=  [<■  +  (rE-f>r)A>  -  (pE-bp)  z„]ju 


Htf,u)  =  -(1  4- A,) 


1  / 


HU 3,i)  =  (H-A,) 


(ItP)u 
1  1 
(i  •/-)« 

W(/9.6p)  =  (I  :  V'  '  -  Z- 
(1  r/2)  U 


H(P,br)=  -(1+A,) 


I  Xj 

(I  f/2)  U 


mp.bt)^  i  o 
//(/S,  A,)  =  tan-1/ 


//(a,  u)  =  -(1  -j-A^)^ 
//(a,  vt)  =  -(I  f- A,) 


denoting  /  =  [*»«  -  (qE~bQ)x1  +  (pE-bp)  y^ju 

L  / 

(1  -t -P)U 
1  1 
(l+P)U 

Hi  a.  bv)  =  (I- A,) — 1 V* 

<1+/2)  u 

H(x,  bp)  —  -(1+A.)  1 

(i  \n  u 

mx'bj  =io 

H( a,  A,)  =  tan  '/ 


//(/»,  /i)  =  1  0 

H(h,bh )  =--  10 


Hit,#)  =  10 
//(<£,  bj  =10 

tf(M)  =  10 

H(8,  bo)  —  10 

W(<M)  =  10 

H(<p,  b^)  —  10 


The  coefficient  of  the  input  matrix  in  Eqn  (B.l)  are  given  below.  The  arguments  in  the  notation 
B(i,j)  denote  the  state  equation  and  input  variable  respectively. 

B(m.Ox)  =  -10 
q)  =  M- 
S(u,  r)  =  — r 

B(i\  ay)  =-10 
B(v,  p)  =  —  w 
B(r,  r)  —  u 


B( H\  Qz)  =  I  0 
B(w,p)  ---  v 
B(w,  q)  -  -u 


B(4>,p)  -  10 
B{<j>,  q)  =  sin  tan  6 
B(<f> ,  r)  =  cos  <!>  tan  6 


B(6 ,  <?)  =  cos  <£ 
fl(0,  r)  --=  —sin  $ 

B(ip,  q)  —  sin  <f>jc os  9 
r)  =  cos  <£/cos  0 


APPENDIX  C 

Example  of  Computer  Program  Set  Up  Data 


RU  CHOICE 


PROCESS  NOISE  VARIANCE 


QUANTITY 

NUMBER 

VALUE 

NX 

1 

000010000 

NY 

2 

0  00160000 

NZ 

3 

1  00000000 

P 

4 

000010000 

Q 

5 

000010000 

R 

6 

0  00010000 

MEASUREMENT 

NOISE  VARIANCE 

VFP 

7 

1  00000000 

BETA 

8 

0- 00002500 

ALPHA 

9 

O' 00002500 

HEGHT 

10 

1000  00000000 

PHI 

II 

0  01000000 

THETA 

12 

0  00250000 

PSI 

13 

0  01000000 

TO  CHANGE  VALUE  TYPE  I,F 
TERMINATE  CHANGES  WITH  0,0 


TYPE  -1,0,1  TO  REVIEW,  TERMINATE.  OR  VIEW  CHANNELS 
I 


INPUT  MEASUREMENTS  AVAILABLE 

QUANTITY 

NUMBER 

STATUS 

1/C  IF 

NOT  MEASURED 

NX 

1 

1 

0  00000000 

NY 

2 

2 

000000000 

NZ 

3 

3 

000000000 

P 

4 

4 

000000000 

Q 

5 

5 

000000000 

R 

6 

6 

000000000 

TO  CHANGE  STATUS  TYPE  1,1  ADD 

—  I,F  TO  CHANGE  I/C 

TERMINATE  CHANGES  WITH  0,0 

MEASUREMENTS  TO 

BE  INCLUDED 

QUANTITY 

NUMBER 

STATUS 

I/C  IF  NOT  MEASURED 

VFP 

1 

1 

0  00000000 

BETA 

2 

2 

0  00000000 

ALPHA 

3 

3 

0  00000000 

HEGHT 

4 

4 

0  00000000 

PHI 

5 

5 

000000000 

THETA 

6 

6 

0  00000000 

PSI 

7 

7 

000000000 

TO  CHANGE  STATUS  TYPE  1,1  ADD 

—  1,F  TO  CHANGE  I/C 

TERMINATE  CHANGES  WITH  0,0 


k 


1 

1 


STATES  TO  BE  ESTIMATED 


STATES  WHICH  CAN 

QUANTITY  NUMBER  STATUS  BE  ESTIMATED  VARIANCE 

CHANGE  AS  BEFORE  ADD  -l,F  TO  CHANGE  VARIANCE 


u 

1 

1 

1  0 

400000000 

V 

2 

2 

1  0 

O' 00250000 

w 

3 

3 

1  0 

0  01000000 

HEGHT 

4 

4 

1  0 

10000000000 

PHI 

5 

5 

10 

0- 00002500 

THETA 

6 

6 

10 

0  00000400 

PSI 

7 

7 

1  0 

0  00000400 

BAX 

8 

0 

1  0 

0- 07700000 

BAY 

9 

0 

1  0 

0- 07700000 

BAZ 

10 

0 

1  0 

0- 07700000 

BP 

II 

0 

1  0 

0 ■ U2286000 

BQ 

12 

0 

1  0 

001940000 

BR 

13 

0 

1  0 

0- 07700000 

BVFP 

14 

0 

1  0 

0  07700000 

BBETA 

15 

0 

1  0 

0- 07700000 

BALFA 

16 

0 

1  0 

0  07700000 

BHGHT 

17 

0 

1  0 

0- 07700000 

BPHI 

18 

0 

10 

0- 07700000 

BTHTA 

19 

0 

10 

0  07700000 

BPSI 

20 

0 

1  0 

0- 07700000 

K.VFP 

21 

0 

10 

0- 07700000 

KBETA 

22 

0 

10 

0- 07700000 

KALFA 

23 

0 

1  0 

0- 07700000 

TYPE  1  OR  0  IF 

REPEAT  IS/IS  NOT  REQUIRED 

PROGRAM  STEPS 


OPERATION 


[1]  Initialise  program 

[1.1]  Read  in  set  up  data 

[1.2]  Read  initial  measurements 

[2]  Initialise  state  estimates 

[3]  Initialise  state  covariance 
estimates 

[4]  Start  filter 


Prepared  by  program  'CHOICE' 


[5]  Output  states  and 
state  variances 


[6]  Calculate  input  matrix 


[7]  State  prediction  by 

numerical  integration  of 
nonlinear  equations 


[8]  Calculation  of  state 
transition  matrix  by 
power  series  approximation 


B  (k  +  1)  and  Hk  +  I) 


[**..,]  -f 

Ui,,] 


kAt 


,AAt 


[9]  Covariance  prediction 


W  (k  +  1)  =  0(k  +  1,  k)  S+(k) 

S  (k  +  1)  =  W(k  +  1)  W  (k  + 1)T +Q(k  + 1)  /a 


FIG.  1  SUMMARY  OF  PROGRAM  STEPS 


PROGRAM  STEPS 


OPERATION 


9.1  <t>ii  x  S+  (k)  11/12 

=  W  (k  +  1)  11/12 

9.2  W(k+1)i2-^P(k  +  1)i2 


9.3  W  (k  +  1)  x  W  (k  +  1)  T 

=  P  (k  +  1 )  ii 

9.4  S+(k)22  -►  P{k  +  1)22 


^W(k  +  1)n  I  W(k  +  1)«J 


P  (k  +  1)ii  I  P  (k  +  1)i2 


x  P(k+  1)22 
S 

\ 

\ 


9.5  Calculation  of  process 
noise  covariance  matrix 


r(k+1)  x  QX(k+1)  x  r(k  +  1)T 


Q  (k  +  1) 


formation  of 

P  (k  +  1)11/12/22  +  Q  (k  +  1) 
=  P  (k  +  1) 


r  '  "1 

P  (k  +  1  )ii  l 


IP  (k  +  1)12 

- L_ - .+  Q  (k  +  1 ) 


P  (k  +  1)22 


P(k  +  1) 


9.6  CHOLESKI  decomposition 


P  (k  +  1) 


10  Read  in  new  measurements 


FIG.  1  CONTINUED 


PROGRAM  STEPS 


[11]  Correction  of  covariance 
and  state  matrices 

S(k  +  1)  and  X(k+1)  using 
CARLSON  algorithm 


[12]  Next  data  point 

S+  (K)  1=  S+  (k  +  1) 
X+(k)  X+(k  +  1) 


Return  to  step  5 


[13]  Calculate  and  output 
bias  errors 


[14]  Calculate  and  output 
state  variances 
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